#pragma once

#include "../utils/kalman_filter.h"

namespace fusion_perception {

class PHVFilter {
public:
    typedef struct MeasuredPosition {
        double x { 0.0 };
        double vx { 0.0 };
        double y { 0.0 };
        double vy { 0.0 };
        double heading { 0.0 };
    } MeasuredPosition;

    typedef struct CorrectedPosition : public MeasuredPosition {
    } CorrectedPosition;

public:
    PHVFilter(void);
    CorrectedPosition correct(double dtS, MeasuredPosition measuredPosition);

private:
    void init(void);
    void predict(double dtS);
    CorrectedPosition correct(MeasuredPosition measuredPosition);

private:
    inline static constexpr const int32_t ds_ { 5 };
    inline static constexpr const int32_t dm_ { 5 };

private:
    bool xInitFlags_ { false };
    KalmanFilter<double, ds_, dm_, 5> kf_;
};
}